﻿#include <errno.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <sys/time.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <pthread.h>
#include <linux/can/raw.h>
#include "car_line.h"
#include "can.h"
#include "can_parse.h"
#include "adas_out.h"

// #include "dbg_thread.h"
#include "printf.h"

#ifndef SetThreadNameOfFuncName
#define SetThreadNameOfFuncName(...)
#endif
#ifndef dbgPrintThreadFuncEnter
#define dbgPrintThreadFuncEnter(...)
#endif
#ifndef dbgPrintThreadFuncExit
#define dbgPrintThreadFuncExit(...)
#endif


//int    exit_thread(void);
static int is_exit = 0;
static int  exit_thread(void)
{
	return is_exit;
}

static adas_out_t local_out;
static int hard_fd = -1;
static struct can_handle can_out_handle = {
	.socket = -1,
	.dev_name = {0,},
};

static int    hard_line_init(struct car_hard_param* param)
{
	hard_fd = open ("/dev/car-line", O_RDONLY);
	if (hard_fd <= 0) {
		printf("Can not open car-line device\n");
		return -1;
	}
	if (param->left_flash_interval)
	    ioctl(hard_fd, CAR_LEFT_TURN_FLASH, param->left_flash_interval);
	if (param->right_flash_interval)
	    ioctl(hard_fd, CAR_RIGHT_TURN_FLASH, param->right_flash_interval);
	return 0;
}

static void    hard_line_release(void)
{
	if (hard_fd > 0)
	    close(hard_fd);
	hard_fd = -1;
}

static int    get_hard_signal(struct car_hard_signal *sig)
{
	if (sig == NULL || hard_fd <= 0)
        return -1;
    if (read(hard_fd, sig, sizeof(*sig)) != 0) 
		return 0;
	return -1;
}

static int    can_out(unsigned int id, unsigned char *data, int bytes)
{
	return can_write(&can_out_handle, id, data, bytes);
}

static struct signal_operations signal_op = {
	.get_hard_line = get_hard_signal,
	.hard_line_init = hard_line_init,
	.hard_line_release = hard_line_release,
	.can_write = can_out,
};
		

	
void*    CarSignal(void * param)
{
	SetThreadNameOfFuncName();			// 调试增加显示线程名,Add by Xsky 2016-07-01
	dbgPrintThreadFuncEnter("");

	car_input_param_t *input = (car_input_param_t*)param;
	const char *ini = "/home/adas/can.xml";
	struct canmsg_filter filter[16];
	struct can_frame frame;
	int nbytes, i, total;
	struct can_handle *handle;
	int ret;
	struct timeval timeout, tv1, tv2;
	fd_set can_set;
	struct signal_ctrl *ctrl;
	unsigned int us;

	if (input != NULL) {
        if (input->cfg_file != NULL) 
			ini = input->cfg_file;
        signal_op.msg_hook = (input->msg_hook == NULL)? NULL : input->msg_hook;
	    signal_op.msg_hook_timeout = (input->msg_hook_timeout == NULL)? NULL : input->msg_hook_timeout;
	}
    if (open_signal_cfg(ini, &signal_op) < 0) {
    	printf("open %s error\n", ini);
    	return 0;
    }
    
    ctrl = get_signal_ctrl();
    if (ctrl == NULL) {
    	close_signal_cfg();
    	return 0;
    }
    
    for (i = 0; i < ctrl->id_num; i++) 
    {
    	filter[i].can_mask = CAN_EXTEND_MASK;
    	filter[i].can_id = ctrl->id_tab[i];
    }
    
    handle = can_open("can0", ctrl->bitrate, ctrl->id_num, filter);
    free(ctrl);
    if (handle == NULL) {
    	close_signal_cfg();
    	return 0;
    } 
    
	can_out_handle = *handle;  /* 复制CAN句柄 */
    total = 0;
    gettimeofday(&tv1, NULL);
    while (1)
    {
        FD_ZERO(&can_set);
        FD_SET(handle->socket, &can_set);
        timeout.tv_sec  = 0;
        timeout.tv_usec = 10000;
    
        ret = select(handle->socket + 1, &can_set, NULL, NULL, &timeout);
        if (ret == 0) {
        	total = 10;
        } else if (ret > 0) {
        	nbytes = read(handle->socket, &frame, sizeof(frame));
        	can_parser(frame.can_id, frame.data);
        	total++;
        }
        
		//#ifndef CREATE_LIB
		if (exit_thread()) {
			printf("exit CarSignal.\n");
        	break; 
		}
		//#endif
        
        if (total >= 5) {
        	total = 0;
            gettimeofday(&tv2, NULL);
            us = (tv2.tv_sec - tv1.tv_sec) * 1000000u + tv2.tv_usec - tv1.tv_usec;
            if (us > 5000) {
                tv1 = tv2;
                can_timeout(us / 1000);
            }
        }
    }

	can_out_handle.socket = -1;
    close_signal_cfg();
	can_close(handle);

	dbgPrintThreadFuncExit("");

	return 0;
}

pthread_mutex_t output_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t output_cond   = PTHREAD_COND_INITIALIZER;
static int output_trig = 0;  /* 是否有输出触发事件 */

obj_out_t object_out = {
	.objs = 0,
	.lane[0].valid = 0,
	.lane[1].valid = 0,
};

void    can_set_output(adas_out_t *output,  obj_out_t *info)
{
	pthread_mutex_lock(&output_mutex);
	if (output)
	    local_out = *output;
	output_trig = 1;
	if (info != NULL && (info->objs > 0 || info->lane[0].valid || info->lane[1].valid) ) 
		object_out = *info;
	pthread_cond_signal(&output_cond);
	pthread_mutex_unlock(&output_mutex);
}


static inline int  float_int(float val)
{
	if (val < 0.0f)
		return (int)(val - 0.5f);
	return (int)(val + 0.5f);
}

static int    object_process(obj_out_t *info, struct can_msg_task *task)
{
	int msgs = 0;
	int val, i;
	unsigned char *dat;
	__object_t *obj;
	__lane_t *lane;
	
	memset(task, 0, sizeof(*task));
	if (info == NULL) {
		if (task) {
			task->msg_count = 1;
			task->msg[0].id = 0x7b0;
		}
		return 1;
	}
	if (task == NULL)
		return 0;
	
	task->msg[0].id = 0x7b0;
	task->msg[0].data[0] = info->objs << 4;
	
	msgs++;
	obj = info->object;
	for (i = 0; i < info->objs && msgs < MAX_MSGS; i++,obj++) 
	{
		task->msg[msgs].id = 0x7b1;
		dat = task->msg[msgs].data;
		
		val = float_int(obj->obj_x * 50 + 2048);
		dat[0] = obj->id | (val << 4);
		dat[1] = val >> 4;
		
		val = float_int(obj->obj_y * 50);
		dat[2] = val;
		dat[3] = ((val >> 8) & 0x1F) | (obj->obj_class << 5);
		
		val = float_int(obj->obj_x_vrel * 10 + 512);
		dat[4] = val;
		dat[5] = (val >> 8) & 3;
		
		val = float_int(obj->obj_y_vrel * 40 + 8192);
		dat[5] |= val << 2;
		dat[6] = val >> 6;
		
		dat[7] = float_int(obj->obj_width * 40);
		msgs++;
	}
	
	lane = info->lane;
	for (i = 0; i < 2 && msgs < MAX_MSGS; i++,lane++)
	{
		dat = task->msg[0].data;
		if (lane->valid == 0)
			continue;
		val = float_int((lane->c3 * (1 << 28) ) + 32768);
		if (i == 0) {
			dat[1] |=1;
		    dat[2] = val;
		    dat[3] = val >> 8;
		} else {
			dat[1] |= 2;
		    dat[4] = val;
		    dat[5] = val >> 8;
		}
		
		task->msg[msgs].id = 0x7b4;
		dat = task->msg[msgs].data;
		if (i == 1)
			dat[0] |= 1;
		dat[0] = (lane->quality << 6) | (lane->type << 2) | i;
		
		val = float_int(lane->c0 * 256 + 32768);
		dat[1] = val;
		dat[2] = val >> 8;
		
		val = float_int(lane->c1 * 1024 + 32768);
		dat[3] = val;
		dat[4] = val >> 8;
		
		val = float_int(lane->c2 * 1024 * 1000 + 32768);
		dat[5] = val;
		dat[6] = val >> 8;
		
		dat[7] = float_int(lane->mark_width * 100);
		msgs++;
	}
	
	task->msg_count = msgs;
	return msgs;
}




static void    clear_object(obj_out_t *info)
{
	if (info == NULL)
		return;
	info->objs = 0;
	info->lane[0].valid = 0;
	info->lane[1].valid = 0;
}


#define OUT_TIMEOUT  10  /* 输出超时时间(ms), 不超过100ms */
void*    CanOutput(void * param)
{
	SetThreadNameOfFuncName();			// 调试增加显示线程名,Add by Xsky 2018-09-14
	dbgPrintThreadFuncEnter("");

	struct can_msg_task can_task = {
	  .msg_count = 0,
    };
	const char *out = (char*)param;
	pthread_mutexattr_t mutex_attr;
    pthread_condattr_t  cond_attr;
	adas_out_t can_output;
	struct timespec timeout;
	int wait, ret, i;
	unsigned long ns;

	memset(&local_out, 0, sizeof(local_out));
	local_out.front_distance = -1;
	local_out.headway = -1;
	
	/* 等待CAN接口打开 */
	wait = 5;
	while (wait > 0)
	{
		if (can_out_handle.socket > 0)
			break;
		wait--;
		usleep(100000);
	}

	if (wait <= 0)
		return 0;
	
	/* 编译CAN输出脚本 */
    if (out == NULL)
    	out = "/home/adas/can.out";
    if (open_adas_script(out, can_out) < 0) {
    	printf("open %s error\n", out);
    	return 0;
    }
	
	/* 初始化条件互斥锁 */
	ret  = pthread_mutexattr_init(&mutex_attr);
 	ret |= pthread_condattr_init(&cond_attr);
 	ret |= pthread_condattr_setclock(&cond_attr, CLOCK_MONOTONIC);
	ret |= pthread_mutex_init(&output_mutex, &mutex_attr);
	ret |= pthread_cond_init(&output_cond, &cond_attr);
	pthread_condattr_destroy(&cond_attr);
	pthread_mutexattr_destroy(&mutex_attr);
	if (ret) {
		printf("Create can output lock error\r\n");
		close_adas_script();
		return 0;
	}
    
	clock_gettime(CLOCK_MONOTONIC, &timeout);
	ns = timeout.tv_nsec + OUT_TIMEOUT * 1000000;
    timeout.tv_sec +=  ns / 1000000000;
    timeout.tv_nsec = ns % 1000000000;
    while (1)
    {
		pthread_mutex_lock(&output_mutex);
		ret = pthread_cond_timedwait(&output_cond, &output_mutex, &timeout);
		can_output = local_out;
		
		if (output_trig) 
		    object_process(&object_out, &can_task);
		clear_object(&object_out);
		output_trig = 0;
		pthread_mutex_unlock(&output_mutex);		
		
		if(ret == ETIMEDOUT) {
			clock_gettime(CLOCK_MONOTONIC, &timeout);
			ns = timeout.tv_nsec + OUT_TIMEOUT * 1000000;
            timeout.tv_sec +=  ns / 1000000000;
            timeout.tv_nsec = ns % 1000000000;
			
			script_execute(&can_output, &can_task, OUT_TIMEOUT);
		} else {
			script_execute(&can_output, &can_task, 0);
		}
        can_task.msg_count = 0;		
		
		//#ifndef CREATE_LIB
		if (exit_thread()) {
			printf("exit CanOutput.\n");
			break;
		}
		//#endif
    }

	pthread_cond_destroy(&output_cond);
	pthread_mutex_destroy(&output_mutex); 
    close_adas_script();

	dbgPrintThreadFuncExit("");

	return 0;
}

static pthread_t can_out_thread = 0;
void can_start_output(const char *script)
{
    pthread_create(&can_out_thread, NULL, CanOutput, NULL);
}

static pthread_t can_in_thread = 0;
void can_start_input(car_input_param_t *input)
{
	static car_input_param_t param;
	if (input != NULL) {
		param = *input;
		input = &param;
	}
    pthread_create(&can_in_thread, NULL, CarSignal, input);
}

#define _DBG_LEVEL		_DBG_WARN
#include "printf.h"

void can_stop(void)
{
	is_exit = 1;

	if (can_in_thread != 0) 
	{
		int ierr = pthread_join(can_in_thread, NULL);
		if (ierr != 0) {
			dbg_error("join CanIn thrd err:%d!\r\n", ierr);
		}
		else {
			can_in_thread = 0;
		}
	}
	
	if (can_out_thread != 0)
	{
		int ierr = pthread_join(can_out_thread, NULL);
		if (ierr != 0) {
			dbg_error("join CanOut thrd err:%d!\r\n", ierr);
		}
		else {
			can_out_thread = 0;
		}
	}

}

